#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <cstring>
#include <arpa/inet.h> // 用于 sockaddr_in 和 inet_ntoa()
#include <signal.h>

class TCPServer
{
public:
    TCPServer(ros::NodeHandle &nh, int port)
        : nh_(nh), port_(port), server_fd_(-1)
    {
        // 创建socket
        if ((server_fd_ = socket(AF_INET, SOCK_STREAM, 0)) < 0)
        {
            ROS_ERROR("Socket creation error");
            return;
        }

        // 设置地址重用
        int opt = 1;
        setsockopt(server_fd_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));

        // 绑定地址
        struct sockaddr_in address{};
        address.sin_family = AF_INET;
        address.sin_addr.s_addr = INADDR_ANY;
        address.sin_port = htons(port_);

        if (bind(server_fd_, (struct sockaddr *)&address, sizeof(address)) < 0)
        {
            ROS_ERROR("Bind failed");
            close(server_fd_);
            return;
        }

        // 开始监听
        if (listen(server_fd_, 1) < 0)
        {
            ROS_ERROR("Listen failed");
            close(server_fd_);
            return;
        }

        ROS_INFO("TCP server listening on port %d", port_);
    }

    ~TCPServer()
    {
        if (server_fd_ != -1)
        {
            close(server_fd_);
        }
    }
    int new_socket;
    void spin()
    {
        ros::Rate rate(10); // 10Hz
        while (ros::ok())
        {
            struct sockaddr_in client_addr{};
            socklen_t client_len = sizeof(client_addr);

            // 接受新连接（非阻塞）
            new_socket = accept(server_fd_,
                                    (struct sockaddr *)&client_addr,
                                    &client_len);

            if (new_socket < 0)
            {
                // 无新连接时继续循环
                rate.sleep();
                continue;
            }

            ROS_INFO("New connection from %s:%d",
                     inet_ntoa(client_addr.sin_addr),
                     ntohs(client_addr.sin_port));

            // 接收数据
            char buffer[1024] = {0};
            while (ros::ok())
            {
                ssize_t bytes_received = recv(new_socket, buffer, sizeof(buffer), 0);

                if (bytes_received <= 0)
                {
                    // 连接关闭或错误
                    std::cout << "Client disconnected" << std::endl;
                    break;
                }
                else
                {
                    // 发布到ROS话题
                    std_msgs::String msg;
                    msg.data = std::string(buffer, bytes_received);
                    pub_.publish(msg);
                    ROS_INFO("Received: %s", buffer);
                    // 清空缓冲区
                    memset(buffer, 0, sizeof(buffer));
                }
            }
            close(new_socket);
        }
    }
    void handle_sigint(int sig)
    {

        close(new_socket);
        close(server_fd_); // 关闭套接字
        ROS_INFO("Shutting down gracefully...");

        ros::shutdown();
    }

private:
    ros::NodeHandle nh_;
    int port_;
    int server_fd_;
    ros::Publisher pub_ = nh_.advertise<std_msgs::String>("tcp_data", 10);
};
static TCPServer *g_server_instance = nullptr;

void sigint_handler_wrapper(int sig)
{
    if (g_server_instance != nullptr)
    {
        g_server_instance->handle_sigint(sig);
    }
}
int main(int argc, char **argv)
{

    ros::init(argc, argv, "tcp_server_node");
    ros::NodeHandle nh;

    int port;
    nh.param<int>("port", port, 8080); // 从参数服务器获取端口，默认8080

    TCPServer server(nh, port);
    g_server_instance = &server;
    signal(SIGINT, sigint_handler_wrapper);
    // signal(SIGINT, server::sigint_handler);
    server.spin();

    return 0;
}